// #include <chrono>
// #include <cmath>
// #include <memory>
// #include <string>
// #include <rclcpp/rclcpp.hpp>
// #include <geometry_msgs/msg/twist_stamped.hpp>
// #include <nav_msgs/msg/odometry.hpp>

// class SpeedSubscriber : public rclcpp::Node
// {
// public:
//     SpeedSubscriber()
//         : Node("speed_subscriber")
//     {
//         // 权重设置
//         declare_parameter<double>("vel_weight", 0.7);
//         declare_parameter<double>("odom_weight", 0.3);
//         declare_parameter<bool>("is_fusion", false);

//         get_parameter("vel_weight", vel_weight_);
//         get_parameter("odom_weight", odom_weight_);
//         get_parameter("is_fusion", is_fusion_);
//         // 初始化速度数据
//         subscription_vel_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
//             "/vel", 10, std::bind(&SpeedSubscriber::vel_callback, this, std::placeholders::_1));
//         subscription_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
//             "/Odometry", 10, std::bind(&SpeedSubscriber::odom_callback, this, std::placeholders::_1));
//         last_vel_valid_ = false; // 初始状态为无效
//         last_odom_valid_ = false;
//     }

// private:
//     void vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
//     {
//         last_vel_x_ = msg->twist.linear.x;
//         last_vel_y_ = msg->twist.linear.y;
//         last_vel_z_ = msg->twist.linear.z;
//         last_vel_valid_ = true;
//         double speed = std::sqrt(last_vel_x_ * last_vel_x_ +
//                                  last_vel_y_ * last_vel_y_ +
//                                  last_vel_z_ * last_vel_z_);
//         RCLCPP_INFO(this->get_logger(),
//                     "/vel 速度: x=%.3f, y=%.3f, z=%.3f, 总体速度=%.3f m/s",
//                     last_vel_x_, last_vel_y_, last_vel_z_, speed);
//         // 输出融合速度
//         if (is_fusion_)
//         {
//             output_fused_speed();
//         }
//     }

//     void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
//     {
//         last_odom_x_ = msg->twist.twist.linear.x;
//         last_odom_y_ = msg->twist.twist.linear.y;
//         last_odom_z_ = msg->twist.twist.linear.z;
//         last_odom_valid_ = true;

//         double speed = std::sqrt(last_odom_x_ * last_odom_x_ +
//                                  last_odom_y_ * last_odom_y_ +
//                                  last_odom_z_ * last_odom_z_);
//         RCLCPP_INFO(this->get_logger(),
//                     "/Odometry 速度: x=%.3f, y=%.3f, z=%.3f, 总体速度=%.3f m/s",
//                     last_odom_x_, last_odom_y_, last_odom_z_, speed);

//         // 输出融合速度
//         if (is_fusion_)
//         {
//             output_fused_speed();
//         }
//     }
//     // 融合速度输出
//     void output_fused_speed()
//     {
//         // 检查是否都有可用数据
//         if (!last_vel_valid_ || !last_odom_valid_)
//             return;

//         // 加权融合
//         double vx = vel_weight_ * last_vel_x_ + odom_weight_ * last_odom_x_;
//         double vy = vel_weight_ * last_vel_y_ + odom_weight_ * last_odom_y_;
//         double vz = vel_weight_ * last_vel_z_ + odom_weight_ * last_odom_z_;
//         double fused_speed = std::sqrt(vx * vx + vy * vy + vz * vz);

//         // 单独速度
//         double vel_speed = std::sqrt(last_vel_x_ * last_vel_x_ +
//                                      last_vel_y_ * last_vel_y_ +
//                                      last_vel_z_ * last_vel_z_);
//         double odom_speed = std::sqrt(last_odom_x_ * last_odom_x_ +
//                                       last_odom_y_ * last_odom_y_ +
//                                       last_odom_z_ * last_odom_z_);

//         RCLCPP_INFO(this->get_logger(),
//                     "[融合速度] /vel速度=%.3f m/s, /Odom速度=%.3f m/s, 融合后=%.3f m/s "
//                     "(vx=%.3f, vy=%.3f, vz=%.3f, 权重: vel=%.2f, odom=%.2f)",
//                     vel_speed, odom_speed, fused_speed, vx, vy, vz, vel_weight_, odom_weight_);
//     }

//     // 权重
//     double vel_weight_;
//     double odom_weight_;
//     bool is_fusion_;
//     // 最近一次/vel数据
//     double last_vel_x_, last_vel_y_, last_vel_z_;
//     bool last_vel_valid_;
//     // 最近一次/odom数据
//     double last_odom_x_, last_odom_y_, last_odom_z_;
//     bool last_odom_valid_;

//     rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr subscription_vel_;
//     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_odom_;
// };

// int main(int argc, char *argv[])
// {
//     rclcpp::init(argc, argv);
//     auto node = std::make_shared<SpeedSubscriber>();
//     rclcpp::spin(node);
//     rclcpp::shutdown();
//     return 0;
// }

// #include <chrono>
// #include <cmath>
// #include <memory>
// #include <string>
// #include <rclcpp/rclcpp.hpp>
// #include <geometry_msgs/msg/twist_stamped.hpp>
// #include <nav_msgs/msg/odometry.hpp>

// class SpeedSubscriber : public rclcpp::Node
// {
// public:
//     SpeedSubscriber()
//         : Node("speed_subscriber")
//     {
//         // 权重设置
//         declare_parameter<double>("vel_weight", 0.7);
//         declare_parameter<double>("odom_weight", 0.3);
//         declare_parameter<bool>("is_fusion", false);

//         get_parameter("vel_weight", vel_weight_);
//         get_parameter("odom_weight", odom_weight_);
//         get_parameter("is_fusion", is_fusion_);
//         // 初始化速度数据
//         subscription_vel_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
//             "/vel", 10, std::bind(&SpeedSubscriber::vel_callback, this, std::placeholders::_1));
//         subscription_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
//             "/Odometry", 10, std::bind(&SpeedSubscriber::odom_callback, this, std::placeholders::_1));
//         last_vel_valid_ = false; // 初始状态为无效
//         last_odom_valid_ = false;
//     }

// private:
//     void vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
//     {
//         last_vel_x_ = msg->twist.linear.x;
//         last_vel_y_ = msg->twist.linear.y;
//         last_vel_z_ = msg->twist.linear.z;
//         last_vel_valid_ = true;
//         double speed = std::sqrt(last_vel_x_ * last_vel_x_ +
//                                  last_vel_y_ * last_vel_y_ +
//                                  last_vel_z_ * last_vel_z_);
//         RCLCPP_INFO(this->get_logger(),
//                     "/vel 速度: x=%.3f, y=%.3f, z=%.3f, 总体速度=%.3f m/s",
//                     last_vel_x_, last_vel_y_, last_vel_z_, speed);
//         // 输出融合速度
//         if (is_fusion_)
//         {
//             output_fused_speed();
//         }
//     }

//     void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
//     {
//         last_odom_x_ = msg->twist.twist.linear.x;
//         last_odom_y_ = msg->twist.twist.linear.y;
//         last_odom_z_ = msg->twist.twist.linear.z;
//         last_odom_valid_ = true;

//         double speed = std::sqrt(last_odom_x_ * last_odom_x_ +
//                                  last_odom_y_ * last_odom_y_ +
//                                  last_odom_z_ * last_odom_z_);
//         RCLCPP_INFO(this->get_logger(),
//                     "/Odometry 速度: x=%.3f, y=%.3f, z=%.3f, 总体速度=%.3f m/s",
//                     last_odom_x_, last_odom_y_, last_odom_z_, speed);

//         // 输出融合速度
//         if (is_fusion_)
//         {
//             output_fused_speed();
//         }
//     }
//     // 融合速度输出
//     void output_fused_speed()
//     {
//         // 检查是否都有可用数据
//         if (!last_vel_valid_ || !last_odom_valid_)
//             return;

//         // 加权融合
//         double vx = vel_weight_ * last_vel_x_ + odom_weight_ * last_odom_x_;
//         double vy = vel_weight_ * last_vel_y_ + odom_weight_ * last_odom_y_;
//         double vz = vel_weight_ * last_vel_z_ + odom_weight_ * last_odom_z_;
//         double fused_speed = std::sqrt(vx * vx + vy * vy + vz * vz);

//         // 单独速度
//         double vel_speed = std::sqrt(last_vel_x_ * last_vel_x_ +
//                                      last_vel_y_ * last_vel_y_ +
//                                      last_vel_z_ * last_vel_z_);
//         double odom_speed = std::sqrt(last_odom_x_ * last_odom_x_ +
//                                       last_odom_y_ * last_odom_y_ +
//                                       last_odom_z_ * last_odom_z_);

//         RCLCPP_INFO(this->get_logger(),
//                     "[融合速度] /vel速度=%.3f m/s, /Odom速度=%.3f m/s, 融合后=%.3f m/s "
//                     "(vx=%.3f, vy=%.3f, vz=%.3f, 权重: vel=%.2f, odom=%.2f)",
//                     vel_speed, odom_speed, fused_speed, vx, vy, vz, vel_weight_, odom_weight_);
//     }

//     // 权重
//     double vel_weight_;
//     double odom_weight_;
//     bool is_fusion_;
//     // 最近一次/vel数据
//     double last_vel_x_, last_vel_y_, last_vel_z_;
//     bool last_vel_valid_;
//     // 最近一次/odom数据
//     double last_odom_x_, last_odom_y_, last_odom_z_;
//     bool last_odom_valid_;

//     rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr subscription_vel_;
//     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_odom_;
// };

// int main(int argc, char *argv[])
// {
//     rclcpp::init(argc, argv);
//     auto node = std::make_shared<SpeedSubscriber>();
//     rclcpp::spin(node);
//     rclcpp::shutdown();
//     return 0;
// }

#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/string.hpp>

class SpeedSubscriber : public rclcpp::Node
{
public:
    SpeedSubscriber()
        : Node("speed_subscriber")
    {
        declare_parameter<double>("alpha", 0.7); // 互补滤波权重
        declare_parameter<bool>("is_fusion", true); // 是否启用互补滤波
        declare_parameter<double>("speed_threshold", 0.05); // 静止/运动阈值

        get_parameter("alpha", alpha_);
        get_parameter("is_fusion", is_fusion_);
        get_parameter("speed_threshold", speed_threshold_);

        subscription_vel_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/vel", 10, std::bind(&SpeedSubscriber::vel_callback, this, std::placeholders::_1));
        subscription_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/Odometry", 10, std::bind(&SpeedSubscriber::odom_callback, this, std::placeholders::_1));

        status_pub_ = this->create_publisher<std_msgs::msg::String>("car_motion_status", 10);

        last_vel_valid_ = false;
        last_odom_valid_ = false;
        fused_vx_ = 0.0;
        fused_vy_ = 0.0;
        fused_vz_ = 0.0;
    }

private:
    void vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
    {
        last_vel_x_ = msg->twist.linear.x;
        last_vel_y_ = msg->twist.linear.y;
        last_vel_z_ = msg->twist.linear.z;
        last_vel_valid_ = true;

        if (is_fusion_)
        {
            output_fused_speed();
        }
        else
        {
            publish_motion_status(last_vel_x_, last_vel_y_, last_vel_z_);
        }
    }

    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        last_odom_x_ = msg->twist.twist.linear.x;
        last_odom_y_ = msg->twist.twist.linear.y;
        last_odom_z_ = msg->twist.twist.linear.z;
        last_odom_valid_ = true;

        if (is_fusion_)
        {
            output_fused_speed();
        }
        else
        {
            publish_motion_status(last_odom_x_, last_odom_y_, last_odom_z_);
        }
    }

    void output_fused_speed()
    {
        if (!last_vel_valid_ || !last_odom_valid_)
            return;

        // 一阶互补滤波
        fused_vx_ = alpha_ * last_vel_x_ + (1 - alpha_) * last_odom_x_;
        fused_vy_ = alpha_ * last_vel_y_ + (1 - alpha_) * last_odom_y_;
        fused_vz_ = alpha_ * last_vel_z_ + (1 - alpha_) * last_odom_z_;

        publish_motion_status(fused_vx_, fused_vy_, fused_vz_);
    }

    void publish_motion_status(double vx, double vy, double vz)
    {
        std_msgs::msg::String status_msg;
        double linear_speed = std::sqrt(vx * vx + vy * vy + vz * vz);

        if (linear_speed < speed_threshold_)
            status_msg.data = "静止";
        else
            status_msg.data = "运动";

        status_pub_->publish(status_msg);
        RCLCPP_INFO(this->get_logger(), "[运动状态] %s (速度=%.3f m/s)", status_msg.data.c_str(), linear_speed);
    }

    double alpha_;
    bool is_fusion_;
    double speed_threshold_;

    double last_vel_x_{0}, last_vel_y_{0}, last_vel_z_{0};
    bool last_vel_valid_;
    double last_odom_x_{0}, last_odom_y_{0}, last_odom_z_{0};
    bool last_odom_valid_;

    // 互补滤波的融合速度
    double fused_vx_, fused_vy_, fused_vz_;

    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr subscription_vel_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_odom_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SpeedSubscriber>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}